package clubhouse.motioncommand;

import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.lang.Math;
import java.util.HashSet;
import java.util.List;
import java.util.ListIterator;
import java.util.Vector;

import clubhouse.base.Base;
import clubhouse.base.Landmarks;
import clubhouse.control.DataChecker;

import clubhouse.voronoi.Pnt;
import clubhouse.voronoi.Triangle;
import clubhouse.voronoi.Triangulation;

/**
 * @author Anis Darwich & Lionel Croix - Simulation and experimentation on Robot in WSN - WnSN Lab - SJTU
 */
public class DecisionMaker {
	/**The value of the distance between the robot and a line above which the robot goes to the line and
	 * under which the robot goes straight to the right bound. Modify this value to change robot behavior.**/
	private static final double robotClosestPtThreshold = 0.2;
	/**The value of a line slope above which this line is considered too vertical and dropped during
	 * decision process. Reduce this value to permit the robot to follow more vertical lines.**/
	private static final double verticalThreshold = 10;
	/**The value of the robot/final destination line slope under which the robot stop following node's range
	 * communication to go straight to the final destination.**/
	private static final double STOPCIRCLETHRESHOLD = 0.1;

	/**
	 * Constructs the DecisionMaker object. If the specified scenario is 2 or 3, initialize the Voronoi diagram 
	 * generation.
	 * @param scenario the current value of simulation's scenario.
	 */
	public DecisionMaker(int scenario){
		if ((scenario == 2)||(scenario == 3)){
	        initialTriangle = new Triangle(
	                new Pnt(-initialSize, -initialSize),
	                new Pnt( initialSize, -initialSize),
	                new Pnt(           0,  initialSize));
	        this.dt = new Triangulation(initialTriangle);
		}
	}
	
	/*
	 * 
	 * 
	 * Scenario 1
	 * 
	 * 
	 * 
	 * 
	 */


	/**
	 * Used only in scenario 1, get the node id of the next destination according to 
	 * current destination and robot position.
	 * @param x_robot the X coordinate of the current robot position.
	 * @param y_robot the Y coordinate of the current robot position.
	 * @param previous_dest the coordinates [X coordinate, Y coordinate] of previous destination.
	 * @param current_index the node id of the current destination (equals to the previous node id destination at 
	 * the beginning and changed to the next destination).
	 * @return the node id of the next destination
	 */
	public int getNextIndex(double x_robot, double y_robot, double[] previous_dest,int current_index){
		double distance = 0;
		if (Base.DEBUG) System.out.println("(getNextIndex) x_robot="+x_robot+", y_robot="+y_robot+", previous_dest[1]="+previous_dest[1]+", previous_dest[2]="+previous_dest[2]);
		distance = Base.euclideanDist(x_robot, y_robot, previous_dest[1], previous_dest[2]);
		if (Base.DEBUG) System.out.println("(getNextIndex) distance robot/previous dest = "+distance);
		if ((distance <= 0.5) && (distance >= -0.5)){
			if (current_index == 4){
				current_index = 1;
			} else {
				current_index = current_index + 1;
			}
		} // else keep the same destination

		return(current_index);
	}
	
	/*
	 * 
	 * 
	 * Scenario 2
	 * 
	 * 
	 * 
	 * 
	 */
	//Scenario 2
	/**Current Delaunay Triangulation*/
    private Triangulation dt;
	/**Initial triangle used in Delaunay Triangulation process*/
    private Triangle initialTriangle;
	/**Size of initial triangle used in Delaunay Triangulation process*/
    private static int initialSize = 1000;
    /**Set of current Voronoi polygons*/
	private Vector<Point2D[]> polygons = null;
    
    /**
     * Get current set of Voronoi polygons.
     * @return the vector of points array representing the set of current polygons, each points array represent
	 * the vertices set of one polygon.
     */
    public Vector<Point2D[]> getPolygons(){
    	return(this.polygons);
    }
    
	 /**
	  * Get the point which is the orthogonal projection of the specified point on the specified line.
	  * @param point the point to project.
	  * @param line the line on which project the specified point.
	  * @return the point resulting of the orthogonal projection of the specified point on the specified line.
	  */
	private Point2D project(Point2D point, Line2D line){
		 Point2D p0 = line.getP1();
		 Point2D p1 = line.getP2();
		 if (point.equals(p0) || point.equals(p1)) return point;
		 double euclDistance = point.distance(p0);	//distance between robot & one bound of the line
		 double d = Math.sqrt(euclDistance*euclDistance-line.ptLineDist(point)*line.ptLineDist(point));	//distance between the projected point & the same bound
		 double xInt,yInt;
		 
		 xInt = p0.getX() + d * Math.cos(Math.atan((p1.getY() - p0.getY())/(p1.getX() - p0.getX())));
		 yInt = p0.getY() + d * Math.sin(Math.atan((p1.getY() - p0.getY())/(p1.getX() - p0.getX())));
		 if (Base.DEBUG) System.out.println("(project) (xInt,yInt) = ("+xInt+","+yInt+")");
		 Point2D coord = new Point2D.Double(xInt,yInt);
		 return coord;

	 }
	 
	/**
	 * Tests if the specified line is in the specified lines vector.
	 * @param vector the vector in which the specified line could be.
	 * @param line the line to test.
	 * @return true if the line is in the vector, one of the line contained in the vector have 
	 * exactly the same bounds than the specified line, ie the same coordinates, false otherwise.
	 */
	private boolean isInside(Vector<Line2D> vector, Line2D line){
		boolean result = false;
		for (int i=0;i<vector.size();i++){
			Line2D vectorLine = vector.get(i);
			result = ((vectorLine.getX1()==line.getX1())&&(vectorLine.getY1()==line.getY1())&&(vectorLine.getX2()==line.getX2())&&(vectorLine.getY2()==line.getY2()));
			if (result) break;
		}
		return result;
	}
	
	/**
	 * Create a line between the 2 specified point in which way that the left bound have the minimal X coordinate
	 * and the right bound have the maximal one.
	 * @param point1 one bound of the line to create.
	 * @param point2 the other bound of the line to create.
	 * @return the line with a left bound with the minimal X coordinate and a right bound with the maximal one.
	 */
	private Line2D createSortedLine(Point2D point1,Point2D point2){
		Line2D result=null;
		if (point1.getX()<=point2.getX()){
			result = new Line2D.Double(point1, point2);
		} else {
			result = new Line2D.Double(point2, point1);
		}
		
		return(result);
	}
	  
	/**
	 * Choose the correct line in all current polygons edges according to the decision algorithm.
	 * @param robot the current robot position.
	 * @return the chosen line to follow.
	 * @throws Exception thrown if an error occur during process, see exception description. Possible errors are:
	 * <br/>- "Cannot find 2 common points", only in the case of only two known landmarks.
	 * <br/>- "Cannot find closest point on the robot's right" during current polygons scan.
	 * <br/>- "Cannot find lines with the closest point" after closest point choice.
	 * <br/>- "Cannot find a correct line where the closest point is the right bound", if this exception occurs
	 * too often, we may should reduce the value of verticalThreshold constant, meaning that the robot could choose a line more 
	 * vertical than what it currently can.
	 * <br/>- "Cannot find a correct line where the closest point is the left bound"
	 * <br/>- "No destLine found", last test before returning chosen line.
	 * <br/>Usually, the most common exception is the 4th one, the other doesn't usually appear.
	 */
	private Line2D getDestLine(Point2D robot) throws Exception {
		Line2D result = null;
		
		int b = 0, i = 0, j;

		if (this.polygons.size() == 2){		//Only 2 polygons, result = common edge = 2 common vertices
			Point2D[] bounds = new Point2D[2];
			if (Base.DEBUG) System.out.println("(getDestLine) Only 2 polygons");

			while ((b<2) && (i<this.polygons.get(0).length)){		//abscissa' loop
				for (int k=0;k<this.polygons.get(1).length;k++){		//ordinates' loop
					if (this.polygons.get(1)[k].equals(this.polygons.get(0)[i])){
						bounds[b] = this.polygons.get(0)[i];
						if (Base.DEBUG) System.out.println("(getDestLine) bounds["+b+"] = "+bounds[b]);
						b++;
						break;
					}
				}
				i++;
			}
			if (b==2){
				result = this.createSortedLine(bounds[0], bounds[1]);
			} else throw new Exception("Cannot find 2 common points");
			
		} else {
			//Get closest point & its 2 lines
			Point2D tempPoint = null, closestPoint=null;
			double tempDist, minimaldist = 10000;
			Vector<Line2D> tempLines = new Vector<Line2D>(5,1);
			
			if (Base.DEBUG) System.out.println("(getDestLine) We have "+this.polygons.size()+" polygons");
			for (i=0;i<this.polygons.size();i++){
				for (j=0;j<this.polygons.get(i).length;j++){
					tempPoint = this.polygons.get(i)[j];
					tempDist = robot.distance(tempPoint);
					
					if (tempDist < minimaldist){
						minimaldist = tempDist;
						closestPoint = tempPoint;
					}
				}//end vertices' for
			}//end polygons' for

			if (closestPoint == null) throw new Exception("Cannot find closest point on the robot's right");
			if (Base.DEBUG) System.out.println("(getDestLine) Closest point : "+closestPoint);
			
			Line2D o = null;
			for (i=0;i<this.polygons.size();i++){
				for (j=0;j<this.polygons.get(i).length;j++){
					if (this.polygons.get(i)[j].equals(closestPoint)){
						if (j==0){	//Be careful with arrays boundaries
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[this.polygons.get(i).length-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[j+1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						} else if (j==this.polygons.get(i).length-1){
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[j-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[0]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						} else {
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[j-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint, this.polygons.get(i)[j+1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						}
					}
				}
			}
			if (tempLines.size() == 0) throw new Exception("Cannot find lines with the closest point");
			
			if (Base.DEBUG) {
				String str = "(getDestLine) ";
				str = str + "The "+tempLines.size()+" lines are : [";
				for (i=0;i<tempLines.size();i++){
					str = str + "[("+tempLines.get(i).getX1()+";"+tempLines.get(i).getY1()+"),("+tempLines.get(i).getX2()+";"+tempLines.get(i).getY2()+")], ";
				}
				System.out.println(str); 
			}
			
			int bestIndex=-1;	
			
			if ((robot.getX()<closestPoint.getX())&&(robot.distance(closestPoint)>DecisionMaker.robotClosestPtThreshold)){ //robot far from the closest point, go to it first
				if (Base.DEBUG) System.out.println("(getDestLine) Robot is far away from the closest point and on its left");
				for (i=0;i<tempLines.size();i++){		//take the line where the closest point is the right bound
					if (tempLines.get(i).getP2().equals(closestPoint)){
						if (tempLines.get(i).getX1()-robot.getX()<=2){
							if (bestIndex==-1) bestIndex = i;
							else {
								if (tempLines.get(i).ptLineDist(robot) < tempLines.get(bestIndex).ptLineDist(robot)) bestIndex = i;
							}
						}
					}
				}
				if (bestIndex == -1 ) throw new Exception("Cannot find a correct line where the closest point ("+closestPoint.toString()+") is the right bound");
				result = tempLines.get(bestIndex);
								
			} else {								//robot close to the closest point, go to the rightest bound
				double slope = 0;
				if (Base.DEBUG) System.out.println("(getDestLine) Robot is on the right of the closest point or close to it");
				for (i=0;i<tempLines.size();i++){		//take the line where the closest point is the left bound
					slope = (tempLines.get(i).getY2() - tempLines.get(i).getY1())/(tempLines.get(i).getX2()-tempLines.get(i).getX1());
					if (Math.abs(slope) > DecisionMaker.verticalThreshold) continue;		//Line too vertical => drop it
										
					//The leftBound is the closestPoint & the closest line
					if ((tempLines.get(i).getP1().equals(closestPoint))){
						if (bestIndex==-1){
							bestIndex = i;
						} else {
							if (tempLines.get(i).ptLineDist(robot)<tempLines.get(bestIndex).ptLineDist(robot)) bestIndex = i;
							//if ( Math.abs((tempLines.get(i).getY1()-tempLines.get(i).getY2())/deltaX) < Math.abs((tempLines.get(indexGoal).getY1()-tempLines.get(indexGoal).getY2())/(tempLines.get(indexGoal).getX1()-tempLines.get(indexGoal).getX2())) ) indexGoal = i;
						}
						
					}
					if (Base.DEBUG) System.out.println("(getDestLine) x_leftBound = "+tempLines.get(i).getX1()+", x_rightBound= "+tempLines.get(i).getX2()+", bestIndex="+bestIndex+", i="+i+", dist="+tempLines.get(i).ptLineDist(robot));
				}
				if (Base.DEBUG) System.out.println("(getDestLine) final bestIndex = "+bestIndex);
				if (bestIndex == -1 )  throw new Exception("Cannot find a correct line where the closest point is the left bound");
				result = tempLines.get(bestIndex);
			}
		}
		if (Base.DEBUG) System.out.println("(getDestLine) The chosen line is : [("+result.getX1()+";"+result.getY1()+"),("+result.getX2()+";"+result.getY2()+")]");	
		if (result == null) throw new Exception("No destLine found");
		return result;
	}

	/**
	 * After found a correct line to follow, get the correct point of this specified line which will be 
	 * the next destination.
	 * @param robot the current robot position.
	 * @param destLine the chosen line to follow.
	 * @return the point which will be the next destination, it could be the specified line's left bound, 
	 * its right bound or the orthogonal projection of robot position on this line according to algorithm.
	 */
	private Point2D getDestPoint(Point2D robot,Line2D destLine) {
		Point2D result = null;
		
		if (Base.DEBUG) System.out.println("(getDestPoint) Distance Robot/Line = "+destLine.ptLineDist(robot));
		if (destLine.ptLineDist(robot) > 1){	//Robot is far away from the line, go to it first
			if (Base.DEBUG) System.out.println("(getDestPoint) Far from the line");
			result = this.project(robot, destLine);
		} else {		//Robot is close to or on the line, go to the right bound
			if (Base.DEBUG) System.out.println("(getDestPoint) Close to the line");
			result = destLine.getP2();
		}
		if (Base.DEBUG) System.out.println("(getDestPoint) result is "+result);
		return result;
	}

	/**
	 * Get the Voronoi polygons from the current Delaunay Triangulation.
	 * @return the vector of points array representing the set of current polygons, each points array represent
	 * the vertices set of one polygon.
	 * @throws Exception thrown if it unable to get any polygons from triangulation.
	 */
	private Vector<Point2D[]> getPolygonsFromTriangulation() throws Exception {
		// Keep track of sites done
        HashSet<Pnt> done = new HashSet<Pnt>(initialTriangle);
        Vector<Point2D[]> polygons = new Vector<Point2D[]>(10,0);
        
        for (Triangle triangle : dt){
            for (Pnt site: triangle) {
                if (done.contains(site)) continue;
                done.add(site);
                List<Triangle> list = dt.surroundingTriangles(site, triangle);
                Point2D[] vertices = new Point2D[list.size()];		//vertices == polygon
                int i = 0;
                for (Triangle tri: list){
                    vertices[i++] = new Point2D.Double(tri.getCircumcenter().coord(0),tri.getCircumcenter().coord(1));
                }
                polygons.add(vertices);
            }
        }
        if (polygons.size() == 0) throw new Exception("Cannot find any polygons");
		return (polygons);
	}

	/**
	 * Generate a new Delaunay Triangulation with all specified landmarks if at least one landmark in 
	 * the specified landmarks set is unknown.
	 * @param landmarks the set of landmarks to add in the Delaunay Triangulation.
	 */
	private void genDelaunayTriang(Vector<double[]> landmarks) {
		Triangle triangle;
		Pnt point;
		int i;
		
		//Generate Delaunay Triangulation
		for(i=0; i < landmarks.size(); i++){
			point = new Pnt(landmarks.get(i)[1], landmarks.get(i)[2]);
			triangle = dt.locate(point);
			if (triangle == null) throw new IllegalArgumentException("No containing triangle");
			if (!triangle.contains(point)){		//point is not in the triangulation => recreate the whole triangulation
				if (Base.DEBUG) System.out.println("(genDelaunayTriang) Landmark " + i + ", on "+ landmarks.size() +", is not in the triangulation");
				dt = new Triangulation(initialTriangle);	//Flush the triangulation
				this.polygons = null;						//Flush current polygons
				for(int j=0; j < landmarks.size(); j++){	//Add all new landmarks
					point = new Pnt(landmarks.get(j)[1], landmarks.get(j)[2]);
					dt.delaunayPlace(point);
				}
				break;
			}	//else : ok, look other landmarks
		}
		
		if (i == landmarks.size()) {	// no new landmark
			if (Base.DEBUG) System.out.println("(genDelaunayTriang) No need to regenerate triangulation");
		} else {
			if (Base.DEBUG) System.out.println("(genDelaunayTriang) We generate a " + dt);
		}
	}
	
	/**
	 * Used in scenario 2&3, get the coordinates of the next destination according to current known landmarks
	 * and robot position.
	 * @param landmarks the current set of all landmarks.
	 * @param x_robot the X coordinate of the current robot position.
	 * @param y_robot the Y coordinate of the current robot position.
	 * @param r_robot the current robot direction in rad, r = 0 means the direction is the x-axis, r follows 
	 * the trigonometric direction.
	 * @return the double array containing the coordinates of next destination [X coordinate, Y coordinate]
	 * @throws Exception thrown if an error occurred during the decision process, check exception initiator function.
	 */
	public double[] getNextPoint(Landmarks landmarks, double x_robot, double y_robot, double r_robot) throws Exception{
		Point2D dest = null;
		Point2D robot = new Point2D.Double(x_robot,y_robot);
		double[] result= new double[2]; 
		double [] tempLandmark = null;
		ListIterator<double[]> li;
		int nbCloseLandmarks = 0;
		
		if (Base.DEBUG) System.out.println("(getNextPoint) Robot : ("+x_robot+","+y_robot+"), r="+r_robot);
		
		//Calculate the number of landmark which the robot is in their communication range	
		li = landmarks.getLandmarks().listIterator();
		double[] closeLandmark = null;
        while(li.hasNext()){
        	tempLandmark = li.next();
        	if (Base.euclideanDist(tempLandmark[1], tempLandmark[2], x_robot, y_robot)<=DataChecker.communicationRange+1){
        		nbCloseLandmarks++;
        		closeLandmark = tempLandmark;
        	}
        }
        
        //Find next destination
        if ((nbCloseLandmarks == 0)||(x_robot>=Base.FINALDESTINATION[0])) {			//Far away from all landmark, go to the final destination
        	if (Base.DEBUG) System.out.println("(getNextPoint) No close landmark, go to ("+Base.FINALDESTINATION[0]+","+Base.FINALDESTINATION[1]+")");
			result = Base.FINALDESTINATION;
        } else if (nbCloseLandmarks == 1){	//Only one landmark close follow the communication range
			if (Base.DEBUG) System.out.println("(getNextPoint) Only landmark "+(int)closeLandmark[0]+" close ("+closeLandmark[1]+","+closeLandmark[2]+")");
			if ((x_robot>closeLandmark[1])&&((Base.FINALDESTINATION[1]-y_robot)/(Base.FINALDESTINATION[0]-x_robot)<=DecisionMaker.STOPCIRCLETHRESHOLD)){	//if on the extrem right of the closeLandmark, stop following the range
				result = Base.FINALDESTINATION;
			} else {
				double distEucl = Base.euclideanDist(x_robot,y_robot,closeLandmark[1],closeLandmark[2]);
				double angle = Math.atan((closeLandmark[2]-y_robot)/(closeLandmark[1]-x_robot)), offset=0;
				do{
					offset = offset + 0.01;
					if (closeLandmark[1]>=x_robot){
						result[0] = -distEucl * Math.cos(angle+offset) + closeLandmark[1];
						result[1] = -distEucl * Math.sin(angle+offset) + closeLandmark[2];
					} else {
						result[0] = distEucl * Math.cos(angle+offset) + closeLandmark[1];
						result[1] = distEucl * Math.sin(angle+offset) + closeLandmark[2];
					}
					} while (Base.euclideanDist(x_robot, y_robot, result[0], result[1])<=1);
			}

		} else {					//Several landmarks, generate Voronoi Diagram then follow it
			if (Base.DEBUG) System.out.println("(getNextPoint) "+ landmarks.size()+" landmarks");
			this.genDelaunayTriang(landmarks.getLandmarks());
			if (this.polygons==null) this.polygons = this.getPolygonsFromTriangulation();
			Line2D destLine = this.getDestLine(robot);
			dest = this.getDestPoint(robot, destLine);
			
			//return destination coordinates in a double[]
			result[0] = dest.getX();
			result[1] = dest.getY();	
		}
		if (Base.DEBUG) System.out.println("(getNextPoint) DestPnt = ("+result[0]+";"+result[1]+")");
		
		return(result);
	}
}
